/**
 * \file
 * \date 18.09.2013
 * \author alex
 */
#include <bullet_rviz_test/visual_object.h>
#include <boost/make_shared.hpp>
#include <bullet/btBulletCollisionCommon.h>
#include <ros/node_handle.h>

namespace bullet_rviz_test
{

int VisualObject::next_marker_id_ = 0;

VisualObject::VisualObject()
{
  marker_id_ = next_marker_id_++;

  ros::NodeHandle nh;
  marker_publisher_ = nh.advertise<visualization_msgs::Marker>(
      "visualization_marker", 10);
}

VisualObject::~VisualObject()
{
}

visualization_msgs::MarkerPtr VisualObject::createMarker(
    const btTransform & world_transform)
{
  visualization_msgs::MarkerPtr marker = boost::make_shared<
      visualization_msgs::Marker>();

  marker->header.frame_id = "/my_frame";
  marker->header.stamp = ros::Time::now();

  marker->ns = "basic_shapes";
  marker->id = marker_id_;
  marker->action = visualization_msgs::Marker::ADD;
  marker->lifetime = ros::Duration();

  btVector3 position = world_transform.getOrigin();
  marker->pose.position.x = position.x();
  marker->pose.position.y = position.y();
  marker->pose.position.z = position.z();

  btQuaternion orientation = world_transform.getRotation();
  marker->pose.orientation.x = orientation.x();
  marker->pose.orientation.y = orientation.y();
  marker->pose.orientation.z = orientation.z();
  marker->pose.orientation.w = orientation.w();

  return marker;
}

void VisualObject::publishMarker(const visualization_msgs::MarkerPtr & marker)
{
  if (ros::ok() && marker_publisher_)
    marker_publisher_.publish(marker);
}

std_msgs::ColorRGBA VisualObject::colorFromRgba(uint32_t rgba)
{
  std_msgs::ColorRGBA color;
  color.r = ((rgba >> 24U) & 0xFFU) / 255.0f;
  color.g = ((rgba >> 16U) & 0xFFU) / 255.0f;
  color.b = ((rgba >> 8U) & 0xFFU) / 255.0f;
  color.a = ((rgba >> 0U) & 0xFFU) / 255.0f;
  return color;
}

}
